#include "stm32f4xx.h"
#include "bsp_SysTick.h"
#include "bsp_led.h"
#include "bsp_voice.h"
#include "bsp_key.h"
#include "lcd_spi_130.h"
#include "bsp_lcd.h"
#include "bsp_adc.h"
#include "bsp_servo.h"
#include "bsp_motor.h"
#include "bsp_vision.h"
#include "bsp_usart.h"
#include "bsp_sensor.h"
#include "bsp_compass.h"
#include "bsp_speaker.h"
#include "bsp_timer.h"
#include "bsp_24cxx.h"
#include "bsp_iic.h"
#include "bsp_mcuid.h"
#include "reset.h"
#include "trace.h"
#include "posture.h"
#include "turn.h"
#include "route.h"
#include "pid_turn.h"
#include "test_route.h"
#include "dk.h"

uint8_t prog;
uint8_t count;
extern float error;

extern int cnt_hdl;
extern int cnt_hdr;

int main(void)
{
    SysTick_Init();
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    /***************前灰度ADC初始化*********************/
    ADC1_Init();
    /***************显示屏初始化************************/
    SPI_LCD_Init();
    /***************PID参数初始化***********************/
    pid_init(&pid_yaw, 55, 10, 4.6, 2, 3);
    Set_PID_turn_params(&pid_comp_params, 2.8, 0, 5, 10);
    /***************传感器初始化************************/
    LED_Init();
    GPIO_HW_Init();
    GPIO_HDLR_Init();
    KEY_Init();
    AT24CXX_Init();
    USART3_COMPASS_Init();
    USART2_Init();
    /***************摄像头初始化************************/
#if OpenMV_ON
    USART1_OpenMV_Init();
#elif MaixCAM_ON
    GPIO_MAIXCAM_Init();
#endif
    /***************语音模块初始化**********************/
#if ONELINE_ON
    GPIO_SPEAKER_Init();
#elif ONELINE_OFF
    GPIO_VOICE_Init();
#endif
    /***************舵机电机初始化**********************/
    TIM5_PWM_Init();
    TIM2_PWM_Init();
    TIM1_Motor_Init();
    /***************巡线定时器初始化********************/
    TIM6_Reset_Init();
    TIM7_Aid_Init();
    TIM3_TurnPID_Init();

    Setup_AllServo();
    // LED_ON;
    delay_ms(100);
    delay_ms(100);
    // prog = Function_Mode();
    // LED_OFF;
    // if (prog == 1) {
    //     servo(5,2300);
    //     stop();
    // }
    // if (prog == 2) {
    //     speed_up(50, 170);
    //     Reset(350, 170);
    //     //Run_delay(190, 1500);
    //     speed_down(170, 50);
    //     stop();
    // }
    // if (prog == 3) {
    //     Reset(1000, 50);
    //     stop();
    // }

    // if (prog == 4) {
    //     Reset(1000, 60);
    //     stop();
    // }
    // if (prog == 5) {
    //     run_delay(50, 0, 1800);
    //     stop();
    // }
    // if (prog == 6) {
    //     while(hdl == 0){
    //         run(190, 190);
    //     }
    //     stop();
    // }
    // if (prog == 7) {

    //     stop();
    // }
    // if (prog == 8) {
    //     ShowsensorsHD();
    // }
    // LED_ON;
    // delay_ms(500);
    count = 0;
    while(1){
        LED_ON;
        delay_ms(300);
        LED_OFF;
        delay_ms(300);
        if (KEY_Scan(0) == 2) break;
    }
    LED_OFF;
    while(1){
        LED_OFF;
        if (KEY_Scan(0) == 2) {
            count += 1;
        }
        if(count == 2) break;
    }
    
    LED_ON;
        ShowsensorsHD();
    while (1) {
    };
}
